
#include "quaternion.h"

void quat_create(float x, float y, float z, float w, struct quaternion* q_out)
{
	q_out->x = x;
	q_out->y = y;
	q_out->z = z;
	q_out->w = w;
}
void quat_create_vector3(const struct vector3* vec, float scalar, struct quaternion* q_out)
{
	q_out->x = vec->x;
	q_out->y = vec->y;
	q_out->z = vec->z;
	q_out->w = scalar;
}
void quat_create_axisangle(const struct vector3* axis, float angle, struct quaternion* q_out)
{
	float single = 0.5f * angle;
	float single1 = sinf(single);
	float single2 = cosf(single);

	q_out->x = axis->x * single1;
	q_out->y = axis->y * single1;
	q_out->z = axis->z * single1;
	q_out->w = single2;
}
void quat_create_rotmatrix(const struct matrix4* matrix, struct quaternion* q_out)
{
	float m11 = matrix->m11 + matrix->m22 + matrix->m33;
	if (m11 <= 0.0f)
	{
		if (matrix->m11 < matrix->m22 || matrix->m11 < matrix->m33)
		{
			if (matrix->m22 <= matrix->m33)
			{
				float single = sqrtf(1.0f + matrix->m33 - matrix->m11 - matrix->m22);
				float single1 = 0.5f / single;
				q_out->x = (matrix->m31 + matrix->m13) * single1;
				q_out->y = (matrix->m32 + matrix->m23) * single1;
				q_out->z = 0.5f * single;
				q_out->w = (matrix->m12 - matrix->m21) * single1;
				return;
			}
			else
			{
				float single2 = sqrtf(1.0f + matrix->m22 - matrix->m11 - matrix->m33);
				float single3 = 0.5f / single2;
				q_out->x = (matrix->m21 + matrix->m12) * single3;
				q_out->y = 0.5f * single2;
				q_out->z = (matrix->m32 + matrix->m23) * single3;
				q_out->w = (matrix->m31 - matrix->m13) * single3;
				return;
			}
		}
		else
		{
			float single4 = sqrtf(1.0f + matrix->m11 - matrix->m22 - matrix->m33);
			float single5 = 0.5f / single4;
			q_out->x = 0.5f * single4;
			q_out->y = (matrix->m12 + matrix->m21) * single5;
			q_out->z = (matrix->m13 + matrix->m31) * single5;
			q_out->w = (matrix->m23 - matrix->m32) * single5;
			return;
		}
	}
	else
	{
		float single6 = sqrtf(matrix->m11 + 1.0f);
		q_out->w = single6 * 0.5f;
		single6 = 0.5f / single6;
		q_out->x = (matrix->m23 - matrix->m32) * single6;
		q_out->y = (matrix->m31 - matrix->m13) * single6;
		q_out->z = (matrix->m12 - matrix->m21) * single6;
		return;
	}
}
void quat_create_yawpitchroll(float yaw, float pitch, float roll, struct quaternion* q_out)
{
	float single = roll * 0.5f;
	float single1 = sinf(single);
	float single2 = cosf(single);
	float single3 = pitch * 0.5f;
	float single4 = sinf(single3);
	float single5 = cosf(single3);
	float single6 = yaw * 0.5f;
	float single7 = sinf(single6);
	float single8 = cosf(single6);

	q_out->x = single8 * single4 * single2 + single7 * single5 * single1;
	q_out->y = single7 * single5 * single2 - single8 * single4 * single1;
	q_out->z = single8 * single5 * single1 - single7 * single4 * single2;
	q_out->w = single8 * single5 * single2 + single7 * single4 * single1;
}
void quat_add(const struct quaternion* q1, const struct quaternion* q2, struct quaternion* q_out)
{
	q_out->x = q1->x + q2->x;
	q_out->y = q1->y + q2->y;
	q_out->z = q1->z + q2->z;
	q_out->w = q1->w + q2->w;
}
void quat_concat(const struct quaternion* q1, const struct quaternion* q2, struct quaternion* q_out)
{
	float x, y, z, w, single, y1, z1, w1, single1, single2, single3, single4;

	x = q2->x;
	y = q2->y;
	z = q2->z;
	w = q2->w;
	single = q1->x;
	y1 = q1->y;
	z1 = q1->z;
	w1 = q1->w;
	single1 = y * z1 - z * y1;
	single2 = z * single - x * z1;
	single3 = x * y1 - y * single;
	single4 = x * single + y * y1 + z * z1;

	q_out->x = x * w1 + single * w + single1;
	q_out->y = y * w1 + y1 * w + single2;
	q_out->z = z * w1 + z1 * w + single3;
	q_out->w = w * w1 - single4;
}
void quat_conjugate(const struct quaternion* q_in, struct quaternion* q_out)
{
	q_out->x = -q_in->x;
	q_out->y = -q_in->y;
	q_out->z = -q_in->z;
	q_out->w = q_in->w;
}
void quat_div(const struct quaternion* q1, const struct quaternion* q2, struct quaternion* q_out)
{
	float x = q1->x;
	float y = q1->y;
	float z = q1->z;
	float w = q1->w;
	float single = q2->x * q2->x + q2->y * q2->y + q2->z * q2->z + q2->w * q2->w;
	float single1 = 1.0f / single;
	float x1 = -q2->x * single1;
	float y1 = -q2->y * single1;
	float z1 = -q2->z * single1;
	float w1 = -q2->w * single1;
	float single2 = y * z1 - z * y1;
	float single3 = z * x1 - x * z1;
	float single4 = x * y1 - y * x1;
	float single5 = x * x1 + y * y1 + z * z1;

	q_out->x = x * w1 + x1 * w + single2;
	q_out->y = y * w1 + y1 * w + single3;
	q_out->z = z * w1 + z1 * w + single4;
	q_out->w = w * w1 - single5;
}
void quat_dot(const struct quaternion* q1, const struct quaternion* q2, float* f_out)
{
	*f_out = q1->x * q2->x + q1->y * q2->y + q1->z * q2->z + q1->w * q2->w;
}
void quat_equals(const struct quaternion* q1, const struct quaternion* q2, int* result)
{
	if (q1->x == q2->x)
	{
		if (q1->y == q2->y)
		{
			if (q1->z == q2->z)
			{
				if (q1->w == q2->w)
				{
					*result = 1;
					return;
				}
				goto ret;
			}
			goto ret;
		}
		goto ret;
	}
	goto ret;
ret: 
	*result = 0;
	return;
}
void quat_inverse(const struct quaternion* q, struct quaternion* q_out)
{
	float x = q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w;
	float single = 1.0f / x;
	
	q_out->x = -q->x * single;
	q_out->y = -q->y * single;
	q_out->z = -q->z * single;
	q_out->w = q->w * single;
}
void quat_len(const struct quaternion* q, float* f_out)
{
	float x = q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w;
	*f_out = sqrtf(x);
}
void quat_lensqr(const struct quaternion* q, float* f_out)
{
	*f_out = q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w;
}
void quat_lerp(const struct quaternion* q1, const struct quaternion* q2, float amt, struct quaternion* q_out)
{
	/* TODO: Optimize by creating local ptr to q_out */

	float single = amt;
	float single1 = 1.0f - single;
	float x = q1->x * q2->x + q1->y * q2->y + q1->z * q2->z + q1->w * q2->w;

	float x1, single2;
	
	if (x < 0.0f)
	{
		q_out->x = single1 * q1->x - single * q2->x;
		q_out->y = single1 * q1->y - single * q2->y;
		q_out->z = single1 * q1->z - single * q2->z;
		q_out->w = single1 * q1->w - single * q2->w;
	}
	else
	{
		q_out->x = single1 * q1->x + single * q2->x;
		q_out->y = single1 * q1->y + single * q2->y;
		q_out->z = single1 * q1->z + single * q2->z;
		q_out->w = single1 * q1->w + single * q2->w;
	}
	quat_lensqr(q_out, &x1);
	single2 = 1.0f / sqrtf(x1);
	q_out->x = q_out->x * single2;
	q_out->y = q_out->y * single2;
	q_out->z = q_out->z * single2;
	q_out->w = q_out->w * single2;
}
void quat_mult(const struct quaternion* q1, const struct quaternion* q2, struct quaternion* q_out)
{
	float x = q1->x;
	float y = q1->y;
	float z = q1->z;
	float w = q1->w;
	float single = q2->x;
	float y1 = q2->y;
	float z1 = q2->z;
	float w1 = q2->w;
	float single1 = y * z1 - z * y1;
	float single2 = z * single - x * z1;
	float single3 = x * y1 - y * single;
	float single4 = x * single + y * y1 + z * z1;

	q_out->x = x * w1 + single * w + single1;
	q_out->y = y * w1 + y1 * w + single2;
	q_out->z = z * w1 + z1 * w + single3;
	q_out->w = w * w1 - single4;
}
void quat_mult_scale(const struct quaternion* q, float scale, struct quaternion* q_out)
{
	q_out->x = q->x * scale;
	q_out->y = q->y * scale;
	q_out->z = q->z * scale;
	q_out->w = q->w * scale;
}
void quat_scale(const struct quaternion* q, float scale, struct quaternion* q_out)
{
	/* Function is same as quat_mult_scale. */
	q_out->x = q->x * scale;
	q_out->y = q->y * scale;
	q_out->z = q->z * scale;
	q_out->w = q->w * scale;
}
void quat_negate(const struct quaternion* q, struct quaternion* q_out)
{
	q_out->x = -q->x;
	q_out->y = -q->y;
	q_out->z = -q->z;
	q_out->w = -q->w;
}
void quat_normalize(const struct quaternion* q, struct quaternion* q_out)
{
	float x = q->x * q->x + q->y * q->y + q->z * q->z * q->w * q->w;
	float temp = 1.0f / sqrtf(x);

	q_out->x = q->x * temp;
	q_out->y = q->y * temp;
	q_out->z = q->z * temp;
	q_out->w = q->w * temp;
}
void quat_slerp(const struct quaternion* q1, const struct quaternion* q2, float amt, struct quaternion* q_out)
{
	float single, single1, single2, single3, single4, x;
	int flag = 0;
	single4 = amt;
	x = q1->x * q2->x + q1->y * q2->y + q1->z * q2->z * q1->w * q2->w;

	if (x < 0.0f)
	{
		flag = 1;
		x = -x;
	}
	if (x <= 0.999999f)
	{
		float single5 = acosf(x);
		float single6 = 1 / sinf(single5);
		single1 = sinf((1.0f - single4) * single5) * single6;

		if (1 == flag)
		{
			single2 = -sinf((single4 * single5)) * single6;
		}
		else
		{
			single2 = sinf((single4 * single5)) * single6;
		}
		single = single2;
	}
	else
	{
		single1 = 1.0f - single4;
		if (1 == flag)
		{
			single3 = -single4;
		}
		else
		{
			single3 = single4;
		}
		single = single3;
	}
	q_out->x = single1 * q1->x + single * q2->x;
	q_out->y = single1 * q1->y + single * q2->y;
	q_out->z = single1 * q1->z + single * q2->z;
	q_out->w = single1 * q1->w + single * q2->w;
}
void quat_subt(const struct quaternion* q1, const struct quaternion* q2, struct quaternion* q_out)
{
	q_out->x = q1->x - q2->x;
	q_out->y = q1->y - q2->y;
	q_out->z = q1->z - q2->z;
	q_out->w = q1->w - q2->w;
}